/**
 * @file iso7816.c
 * @brief T1 ISO7816 Driver
 *
 * @author luoxl@witsi.cn
 * @date 2013-11-15
 * @version T1.ISO.01
 */
#include "arch.h"
#include "config.h"
#include "ictl.h"
#include "iodev.h"
#include "syscfg.h"
#include "autoconfig.h"
#include "kernel.h"
#include <string.h>
#include <sys/ioctl.h> 
#include "iso7816.h"
#include "clk.h"
#include "config.h"

#ifndef CONFIG_ICC0_CLK
#define CONFIG_ICC0_CLK 1000000
#endif

#ifndef CONFIG_ICC1_CLK 
#define CONFIG_ICC1_CLK 1000000
#endif

#ifndef CONFIG_ICC2_CLK
#define CONFIG_ICC2_CLK 1000000
#endif

#define ICC_5V_CFG()     do{SYSCFG->ISO_CTL &= ~(ISO0_M_IO_ES_MSK | ISO0_M_IO_5V);\
	                          SYSCFG->ISO_CTL |= (ISO0_M_IO_ES(3) | ISO0_M_IO_5V);\
	                          SYSCFG->ISO_CTL &= ~ISO0_VCC_3V; SYSCFG->ISO_CTL |= ISO0_VCC_5V;\
                            }while(0)
#define ICC_3V_CFG()     do{SYSCFG->ISO_CTL &= ~(ISO0_M_IO_ES_MSK | ISO0_M_IO_5V);\
	                          SYSCFG->ISO_CTL |= ISO0_M_IO_ES(3);\
	                          SYSCFG->ISO_CTL &= ~ISO0_VCC_5V; SYSCFG->ISO_CTL |= ISO0_VCC_3V;\
                            }while(0)                            
#define ICC_1V8_CFG()     do{SYSCFG->ISO_CTL &= ~(ISO0_M_IO_ES_MSK | ISO0_M_IO_5V);\
	                           SYSCFG->ISO_CTL |= ISO0_M_IO_ES(5);\
                            }while(0)                            


static IPNum_t g_ipNum = IP0;
static icc_reg_t *icc = ISO0;

static uint32_t timer_stamp0, timer_stamp1; /* CP0 timer stamp */
static uint32_t icc_timer_factor = CONFIG_SYS_TIMER_FREQ /CONFIG_ICC0_CLK;

#define TIMER2CLOCK(timerVal) ((timerVal)/(icc_timer_factor)) /* change timer value into icc clocks */
#define COUNT_PER_ETU ((g_atr[g_ipNum].F/g_atr[g_ipNum].D) * icc_timer_factor)
/* note: wait cp0 clocks, not wait icc clocks */
static void wait_clocks(clocks)
{
	uint32_t temp = get_sys_timer();
	while((get_sys_timer() - temp) < clocks);
}

#define ATR_RST_LOW_DURATION 42000
#define ATR_FIRST_BYTE 42000
#define ATR_DURATION_CLOCKS_MAX  7499520 /* 20160*372 clocks */
#define ATR_BYTE_INTERVAL_MAX    3749760 /* 10080etus */

typedef struct {
	unsigned long WWT;
	unsigned long CWT;
	unsigned long BWT;
	unsigned int F;
	unsigned char D;
	unsigned char IFSC;
	unsigned char NAD;
	unsigned char GTR;
	unsigned char wtx;
	unsigned char T;

	unsigned char IFD_Seq;
	unsigned char ICC_Seq;
	unsigned char IFSD_Negotiated;
}atr_t;

static atr_t g_atr[3];
static unsigned char t1_parity_error = 0;
static unsigned char t1_sblock_sent = 0;

#define SPECIFIC_MODE_USE_DEFAULT_F_D   1      /* Use default iso7816  F and D value */
#define SPECIFIC_MODE_USE_TA1_F_D       2      /* Use Card ATR TA1 byte F and D value */
#define NEGOTIABLE_MODE                 3      /* Card is in Negotiable mode */
#define wait_t0_BGT() wait_clocks(16*COUNT_PER_ETU)
#define wait_t1_BGT() wait_clocks(22*COUNT_PER_ETU)

static inline void icc_poweron(void)
{
	switch (g_ipNum){
		case IP0:
			SYSCFG->ISO_CTL |= ISO0_VCC_EN;
			break;
			
		case IP1:
			SYSCFG->ISO_CTL |= ISO1_VCC_EN;
			break;
			
		case IP2:
			SYSCFG->ISO_CTL |= ISO2_VCC_EN;
			break;
			
		default:
			break;
	}
}

static inline void icc_poweroff(void)
{
	switch (g_ipNum){
		case IP0:
			SYSCFG->ISO_CTL &= ~ISO0_VCC_EN;
			break;
			
		case IP1:
			SYSCFG->ISO_CTL &= ~ISO1_VCC_EN;
			break;
			
		case IP2:
			SYSCFG->ISO_CTL &= ~ISO2_VCC_EN;
			break;
			
		default:
			break;
	}	   
}

static inline void icc_clock_start(void)
{
	SYSCFG->ISO_CTL |= (ISO0_TX_CLK_EN | ISO0_RX_CLK_EN)<<g_ipNum;
}

static inline void icc_clock_stop(void)
{
	SYSCFG->ISO_CTL &= ~((ISO0_TX_CLK_EN | ISO0_RX_CLK_EN)<<g_ipNum);
}

static inline void icc_rst_h(void)
{
	SYSCFG->RST_SOFT0 &= ~(ISO0_EX_RST<<g_ipNum);
}

static inline void icc_rst_l(void)
{
	SYSCFG->RST_SOFT0 |= ISO0_EX_RST<<g_ipNum;
}

static inline void icc_dat_h(void)
{
	if (g_ipNum == IP0) {
		return;
	}
	else if (g_ipNum == IP1) {
		SYSCFG->ISO_CTL |= ISO1_IO_MOD;
		SYSCFG->ISO_CTL |= ISO1_IO_DATA;
	}
	else if (g_ipNum == IP2) {
		SYSCFG->ISO_CTL |= ISO2_IO_MOD;
		SYSCFG->ISO_CTL |= ISO2_IO_DATA;		
	}
}

static inline void icc_dat_l(void)
{
	if (g_ipNum == IP0) {
		return;
	}
	else if (g_ipNum == IP1) {
		SYSCFG->ISO_CTL &= ~ISO1_IO_MOD;
		SYSCFG->ISO_CTL &= ~ISO1_IO_DATA;		
	}
	else if (g_ipNum == IP2) {
		SYSCFG->ISO_CTL &= ~ISO2_IO_MOD;
		SYSCFG->ISO_CTL &= ~ISO2_IO_DATA;			
	}
	udelay(50);
}

static inline void icc_rst_cfg(unsigned int width)
{
	switch (g_ipNum) {
		default:
		case IP0:
			SYSCFG->ISO_RST_W &= ~0x0000ffff;
			SYSCFG->ISO_RST_W |= width;
			break;
			
		case IP1:
			SYSCFG->ISO_RST_W &= ~0xffff0000;
			SYSCFG->ISO_RST_W |= width<<16;			
			break;
			
		case IP2:
			SYSCFG->ISO_RST_W2 = width;			
			break;
	}
}

void ip7816_rst(void)
{
	SYSCFG->RST_SOFT0 |= (ISO0_EX_RST | ISO0_RST)<<g_ipNum;
	SYSCFG->RST_SOFT1 |= ISO0_PRST<<g_ipNum;
	mdelay(1);
	SYSCFG->RST_SOFT0 &= ~((ISO0_EX_RST | ISO0_RST)<<g_ipNum);
	SYSCFG->RST_SOFT1 &= ~(ISO0_PRST<<g_ipNum);	
	mdelay(5);
}

void auto_poweronoff_enable(void)
{
	if (g_ipNum == IP0) {
		SYSCFG->ISO_CTL |= HW_PD_EN;
	}
}

void auto_poweronoff_disable(void)
{
	if (g_ipNum == IP0 ) {
		SYSCFG->ISO_CTL &= ~HW_PD_EN;
	}
}

static int icc_present(void)
{
	int ret = 1;
	if (g_ipNum == IP0) {
		ret = SYSCFG->ISO_CTL & ISO0_NOCARD ? 0 : 1;
	}   
	return ret;
}


/*
** BRIEF:
**		sci7816 init
** PARAM:
**    void
** RETURN:
**	  null
*/
static void ip7816_init(void)
{
	ip7816_rst();
	auto_poweronoff_disable();
	SYSCFG->CLK_SWT &= ~ISO_CLK_PLL; /* FCLK2 select FOSC source */
    SYSCFG->CLK_DIV2 = ((CONFIG_MOSC/CONFIG_ICC0_CLK)-1)\
                     |(((CONFIG_MOSC/CONFIG_ICC1_CLK)-1)<<9)\
                     |(((CONFIG_MOSC/CONFIG_ICC2_CLK)-1)<<18); 
	icc->SCIMODHW = 0x81;               /* T=1, 4 retry tx, direct convention */
	icc->SCICTRL = 0x0a;                /* H/W IO PAD1, retry tx enable */
	icc->WTCTRL= 0x03;                  /* CWT enable , BWT enable */
	icc->EDCCTRL = 0x01;                /* EDC calculate (T=1) enable */
	icc->ETUDATA = 372;                 /* 1ETU = 372 clocks */
	icc_rst_cfg(ATR_RST_LOW_DURATION);  /* rst keep low 42,000 cycles */
	ICC_5V_CFG();
}

static void apb7816_powerup(void)
{
	icc_poweron();
	udelay(100);
	icc_rst_h();
	udelay(500);
	icc_clock_start();
	icc_dat_h();
	auto_poweronoff_enable();
}

/*
** BRIEF:
**		icc power down
** PARAM:
**    void
** RETURN:
**		null
*/
static void icc_powerdown(void)
{
	auto_poweronoff_disable();

	icc_rst_l();
	udelay(50);
	icc_clock_stop();
	udelay(50);
	icc_dat_l();
	icc_poweroff();

}

static void icc_warm_reset(void)
{
	icc->SCIMODHW = 0x81;         /* T=1, 4 retry tx, direct convention */
	icc->ETUDATA = 372;           /* 1ETU = 372 clocks */
	icc->SCICTRL |= 0x30;         /* clear rx/tx fifo */
	icc->SCIINTIO1 = 0;           /* clear rx/tx int */
	icc_rst_l();
	udelay(1);
	icc_rst_h();
}

static int atr_get_first_byte(void)
{
	while(icc->SCIINTRST != 1) {
		if (TIMER2CLOCK(get_sys_timer() - timer_stamp0) > ATR_FIRST_BYTE) return ERR_ATR_START_TIMEOUT;
	}
	icc->SCIINTRST = 0;
	while ((icc->SCISTAT&ICC_RX_FIFO_NOT_EMPTY) != ICC_RX_FIFO_NOT_EMPTY) {
		if (TIMER2CLOCK(get_sys_timer() - timer_stamp0) > ATR_FIRST_BYTE) return ERR_ATR_START_TIMEOUT;
		if (icc->SCISTAT&ICC_RX_ERROR) return ERR_RECV_PARITY;
	}
	timer_stamp0 = get_sys_timer();/* ATR between two consecutive characters
                                    10,080 initial etus (9,600 + 480 initial etus) */
	timer_stamp1 = timer_stamp0; /* ATR having a duration <= 20,160 initial etus */
                               /* note: do not modify timer_stamp1 until the end of ATR */
	return (int) icc->SCIBUFHW;
}

static int atr_get_next_byte(void)
{
	while ((icc->SCISTAT&ICC_RX_FIFO_NOT_EMPTY) != ICC_RX_FIFO_NOT_EMPTY) {
		if (TIMER2CLOCK(get_sys_timer() - timer_stamp0) > ATR_BYTE_INTERVAL_MAX) return ERR_ATR_BYTE_TIMEOUT;
		if (TIMER2CLOCK(get_sys_timer() - timer_stamp1) > ATR_DURATION_CLOCKS_MAX) return ERR_ATR_TIMEOUT;			
		if (icc->SCISTAT&ICC_RX_ERROR) return ERR_RECV_PARITY;
	}
	timer_stamp0 = get_sys_timer();
	return (int) icc->SCIBUFHW;
}

static int atr_get_t0_tck(void)
{
	wait_clocks(24*COUNT_PER_ETU);
	if (icc->SCISTAT&ICC_RX_FIFO_NOT_EMPTY) {
		return (int) icc->SCIBUFHW;
	}
		return -1;
}



/* return:
**  > 0 atr bytes
**  = 0 reject atr
**  < 0 error
*/
int icc_atr_sequence(unsigned char warm, unsigned char * atrstr, int buflen)
{
	int result;
	unsigned char cur_byte, done = 0, index = 0, check = 0;
	unsigned char historicalBytes = 0;
	unsigned char expectedCharacters = 0;
	unsigned char reject_atr=0;
	unsigned char TCK, TC2 = 0x0A, TB1=0, atrmode = NEGOTIABLE_MODE;
	unsigned int TB3 = -1;
	unsigned char interfaceIteration = 1;
	unsigned char works[256];

	/* default value */
	g_atr[g_ipNum].F = 372;
	g_atr[g_ipNum].D = 1;
	g_atr[g_ipNum].IFSC = 0x20;
	g_atr[g_ipNum].NAD = 0;
	g_atr[g_ipNum].CWT = 11 + (1 << 13);
	g_atr[g_ipNum].BWT = 11L + (1L << 4L) * 960L;
	g_atr[g_ipNum].GTR = 0;
	g_atr[g_ipNum].T = 0;

	/* Wait 40000 to 45000 cycles to release reset, 40000 + N
	   it depend on interface chip, we add N to timer0
	*/
	wait_clocks(ATR_RST_LOW_DURATION*icc_timer_factor);
	
	/* open no later than 380 clocks, closed no earlier than 42000 clocks, after T1
	   if no response from ICC, deactivate it no earlier than 42001 clocks, no later
	   than 42000 clocks + 50ms, after T1,
	   ICC ATR start at 400 and 40000 clocks, after T1
	*/
	timer_stamp0 = get_sys_timer(); /* First ATR should be within 42000 */

	if ((result = atr_get_first_byte()) < 0) {
		return result; /* TS Character */
	}
	icc->SCIMODHW &= 0xfffe; /* set T=0, so IP can check parity */

	cur_byte = result;
	if (cur_byte == 0x03) {
		icc->SCIMODHW |= ICC_dirc; /* inverse convention */
		cur_byte = 0x3f;
	}
	else if (cur_byte != 0x3B && cur_byte != 0x3F) {
		return ERR_ATR_TS;
	}

	works[index ++] = cur_byte;
	while (!done) {
 		if ((result = atr_get_next_byte()) < 0) {
			return result;
 		}
 		if (index==1 && (result == 0x03 || result == 0x3b)) { /* we maybe receive a invalid byte which is the same with*/
 			continue;                                           /* the first byte when T1 mode changed into T0 mode */
 		}
		cur_byte = result;
		works[index ++] = cur_byte;
		check ^= cur_byte;

		if (index != 2) {
			if (expectedCharacters & 0x10) /* TA */ {
				expectedCharacters &= 0xE0;
				switch (interfaceIteration) {
					case 1:
						/* TA1 : [Fi:Di]
						   If TA2 is return with b5 = 0, accept 0x11<= TA1 <= 0x13,
						   or reject it unless we are able to
						   If TA2 isn't returned, use default
						*/
						g_atr[g_ipNum].D = cur_byte;
						break;
					case 2: /* TA2 */
						if ((cur_byte & 0x0F) != g_atr[g_ipNum].T) reject_atr = 1; /* indicates the protocol to be used */
						else if (cur_byte & 0x10) reject_atr = 1; /* b5 should = 0 */
						else if (g_atr[g_ipNum].D !=1 ) /* if TA1 is returned */{
							/* put TA1 test here, to see if TA2 is present */
							if (g_atr[g_ipNum].D < 0x11 || g_atr[g_ipNum].D > 0x13) {
								reject_atr = 1;
							}
							else {
								if ((g_atr[g_ipNum].D &= 0x0F) == 3) g_atr[g_ipNum].D = 4;
								atrmode = SPECIFIC_MODE_USE_TA1_F_D; /* use specific F/D */
							}
						}
						break;
					case 3: /* TA3 */
						/* The terminal shall reject an ATR containing TA3 having a
							value in the range '00' to '0F' or a value of 'FF', default 0x20 */
						if(((cur_byte >= 0x00) && (cur_byte < 0x10))|| (cur_byte == 0xff)){
							reject_atr = 1;
						}
						g_atr[g_ipNum].IFSC = cur_byte;
						break;
				}
			}
			else if (expectedCharacters & 0x20) /* TB */ {
				expectedCharacters &= 0xD0;
				if (interfaceIteration == 1) { /* TB1 */
					/* code reset only accept TB1 = 0x00 */
					if (!warm && cur_byte != 0) reject_atr = 1;
					TB1 = 1;
				}
				else if (interfaceIteration == 3) TB3 = cur_byte; /* TB3 checked below */
				else if (interfaceIteration == 2) reject_atr = 1; /* The terminal shall reject an ATR containing TB2 */
			}
			else if (expectedCharacters & 0x40) /* TC */ {
				 expectedCharacters &= 0xB0;
				 switch (interfaceIteration) {
				 	case 1: /* TC1 */
						if (!warm && !TB1) reject_atr = 1;
						else g_atr[g_ipNum].GTR = cur_byte;
						break;
					case 2: /* TC2 */
						if (cur_byte != 0x0A) reject_atr = 1;
						else TC2 = cur_byte; /* accept an ATR containing TC2 = '0A' */
						break;
					case 3: /* TC3 */
						if (cur_byte != 0x00) reject_atr = 1; /* only 0x00 accpeted */
						break;
				 }
			}
			else if (expectedCharacters == 0x80) /* TD */ {
				 expectedCharacters = (cur_byte & 0xF0);
				 if (interfaceIteration == 1) {
				 	if((cur_byte & 0xF) > 1) reject_atr = 1; /* the l.s. nibble having a value of '0' or '1' */
					if((!warm) && (!TB1)) reject_atr = 1;
				 	g_atr[g_ipNum].T = cur_byte & 0x0F;
				 }
				 else if (interfaceIteration == 2) {
				 	if((((cur_byte & 0x0F) | g_atr[g_ipNum].T) != 0xE) && ((cur_byte & 0x0F) != 1)) reject_atr = 1;
				 }
				 interfaceIteration ++;
			}
			else if (expectedCharacters == 0x02) /* TCK */ {
				TCK = cur_byte;
				done = 1;
			}
			else if (expectedCharacters == 0x01) /* Historical Bytes */ {
				if (-- historicalBytes == 0) {
					if (g_atr[g_ipNum].T == 0) done = 1; /* only T=1 require checksum */
					else expectedCharacters = 0x02; /* checksum, require TCK */
				}
			}
			/* if no historical bytes required, check if TCK needed */
			if (expectedCharacters == 0) {
				if (historicalBytes == 0) {
					if (g_atr[g_ipNum].T == 0) done = 1; /* only T=1 require checksum */
					else expectedCharacters = 0x02; /* checksum, require TCK */
				}
				else expectedCharacters = 0x01;
			}
		}
		else{ /* T0 Character */
			historicalBytes = cur_byte & 0x0F;            /* Find out historical bytes */
			expectedCharacters = cur_byte & 0xF0;         /* Figure out upcoming protocol conf bytes */
		}
	}
	if (g_atr[g_ipNum].T == 0) {
		if ((result = atr_get_t0_tck()) >= 0) /* T0 send TCK */ {
			cur_byte = result;
			works[index ++] = cur_byte;
			check ^= cur_byte;
			if ((check != 0) && (reject_atr == 0)) return ERR_ATR_TCK_T0; /* FCS error */
		}
	}
	else {
		if ((check != 0) && (reject_atr == 0)) return ERR_ATR_TCK_T1; /* FCS error */
		if ((result = g_atr[g_ipNum].GTR) == 0xFF) result = -1;
		if((TB3 == -1) || ((TB3 & 0x0F) > 0x05) ||
			((TB3 & 0xF0) > 0x40) || ((1 << (TB3 & 0x0f)) <= (result + 1))) {
			reject_atr=1;
		}
	}
	if(reject_atr == 1) {
		return 0;
	}

	if (g_atr[g_ipNum].GTR == 0xFF) g_atr[g_ipNum].GTR = 0;
		icc->SCIMODHW &= 0x00ff; /* CGT_CFT mask */
		icc->SCIMODHW |= g_atr[g_ipNum].GTR<<8;
	if (atrmode == SPECIFIC_MODE_USE_TA1_F_D) {
		icc->ETUDATA = g_atr[g_ipNum].F/g_atr[g_ipNum].D;
	}
	else {
		g_atr[g_ipNum].D = 1;
	}

	g_atr[g_ipNum].WWT = g_atr[g_ipNum].D * (TC2 * 960L + 480L);

	if (TB3 != -1) {
		g_atr[g_ipNum].CWT = 11 + (1 << (TB3 & 0x0F));
		g_atr[g_ipNum].BWT = 11L + (1L << ((TB3 & 0xF0) >> 4)) * 960L * g_atr[g_ipNum].D;
	}
	if(g_atr[g_ipNum].T == 1) {
		icc->SCIMODHW |= ICC_tmode;
		g_atr[g_ipNum].BWT += 960L * g_atr[g_ipNum].D;
		g_atr[g_ipNum].CWT += 4;
		g_atr[g_ipNum].IFD_Seq = g_atr[g_ipNum].ICC_Seq = g_atr[g_ipNum].IFSD_Negotiated = 0;
	}
	else {
		icc->SCIMODHW &= ~ICC_tmode;
	}
	if (atrstr) {
		if (buflen < index) index = buflen;
		memcpy(atrstr, works, index);
	}
	return index;
}

int write_t0_byte(unsigned char onebyte)
{
	icc->SCIBUFHW = onebyte;
	while (icc->SCISTAT & ICC_TX_FIFO_NOT_EMPTY){
		if (icc->SCISTAT & ICC_TX_ERROR) return ERR_SEND_PARITY;
	}
	if (icc->SCISTAT & ICC_TX_ERROR) return ERR_SEND_PARITY;
	return (0);
}

int write_t0_last_byte(unsigned char onebyte)
{
	icc->SCIBUFHW = onebyte;
	while (icc->SCISTAT & ICC_TX_FIFO_NOT_EMPTY){
		 if (icc->SCISTAT & ICC_TX_ERROR) return ERR_SEND_PARITY;
	}
	if (icc->SCISTAT & ICC_TX_ERROR) return ERR_SEND_PARITY;
	timer_stamp0 = get_sys_timer(); /* WWT */
	return (0);
}

int read_t0_byte(void)
{
	while(1)
	{
		if ((get_sys_timer() - timer_stamp0) > g_atr[g_ipNum].WWT*COUNT_PER_ETU) return ERR_RECV_TIMEOUT;
		if ((icc->SCISTAT&ICC_RX_FIFO_NOT_EMPTY) != ICC_RX_FIFO_NOT_EMPTY) {
			continue;
		}
		timer_stamp0 = get_sys_timer();
		return (int)icc->SCIBUFHW;
	}
}

int icc_send_apdu0(unsigned char *buffer, unsigned int length, unsigned char *rbuffer)
{
	int index, retval;
	unsigned char * hdr = buffer;
	unsigned char INS, P3 = 0, wait_for_proc_byte;
	unsigned char SW1 = 0, SW2 = 0, warning_status_byte = 0,case4_cmd = 0, get_resp = 0, inverse_INS;
	unsigned int rindex = 0, Lc = 0, Le = 0, val;
	unsigned char  GET_RESPONSE[5] = {0,0xC0,0,0,0};

	/* identify command type */
	if (length > 5) {
		Lc = buffer[4];
		if(length > Lc + 5) {
		      Le = buffer[length - 1];
		      case4_cmd = 1;
		}
		P3 = Lc;
	}
	else if(length == 5) {
		Le = buffer[4];
		P3 = Le;
	}
  
  /* Follow BGT time before sending data at opposite direction */
  wait_t0_BGT();
  
	while (1) {
		INS = hdr[1];
    inverse_INS = ~INS;
    wait_for_proc_byte = 1;

		/* write 5 byte command header */
		for (index = 0; index < 4;) {
		    if ((retval= write_t0_byte(hdr[index ++])) < 0) {
					return retval;
		    }
		}
		if ((retval = write_t0_last_byte(P3)) < 0) {
			return retval;
		}
		index = 5;
		while (wait_for_proc_byte) {
			if ((retval = read_t0_byte()) < 0) {
				return retval;
			}
			val = retval;
			switch (val) {
				case 0x61: /* Card wants GET Response to get APDU response */
					get_resp = 1;
				case 0x6C: /* Card wants to repeat APDU header with expect APDU response byte */
					Lc = 0;
					hdr = (val == 0x61) ? GET_RESPONSE:buffer;
					P3 = Le = retval = read_t0_byte ();
					Le += rindex;
					if(retval < 0) return retval;
					wait_for_proc_byte = 0;
					wait_t0_BGT();
				case 0x60: /* Card wants to have additional WWT */
					break;
				default:
					if((val == INS) || (val == inverse_INS)) { /* all remaining data or next data byte */
						if (Lc + 5 == index) { /* LC have been send out */
							/* receive data from ICC */
							while((Le > rindex) && ((retval = read_t0_byte()) >= 0)) {
								rbuffer[rindex ++] = retval;
								if(val == inverse_INS) {
									break;
								}
							}
							if (retval < 0) return retval;
							break; /* wait for next proc byte or status byte */
						}
						wait_t0_BGT();

						/* if we received INS byte, send all remaining bytes, otherwise, send only one byte */
						if(val == INS) {
							for (;index < (Lc + 4);index ++) {
								if ((retval = write_t0_byte(buffer[index])) < 0) return retval;
							}
						}

						if ((retval = write_t0_last_byte(buffer[index ++])) < 0) return retval;
						break; /* wait for next proc byte or status byte */
					}

					if (((val & 0xf0) != 0x60) && ((val & 0xf0) != 0x90)) return ERR_INVALID_SW;
					if ((retval = read_t0_byte()) < 0) return retval; /* SW2 */
					if (!warning_status_byte) {
						SW2 = retval;
            SW1 = val;
						/* Check status byte to see whether card has more data to read */
						if((((SW1 == 0x62) || (SW1 == 0x63)) ||
							(((SW1 & 0x90) ==0x90) && (!((SW1 == 0x90) && (SW2 == 0))))) &&
							((Lc + 5) == index) && (case4_cmd) && (get_resp == 0)) {
							warning_status_byte = 1;

							Lc = 0;
							hdr = buffer = GET_RESPONSE;
							P3 = Le = 0;
							Le += rindex;
							wait_for_proc_byte = 0;
							wait_t0_BGT();
						}
						else {  /* Card completed APDU response, send response with Status code */
							rbuffer[rindex ++] = SW1;
							rbuffer[rindex ++] = SW2;
							return rindex;
						}
					}
					else {
						rbuffer[rindex ++] = SW1;
						rbuffer[rindex ++] = SW2;
						return rindex;
					}
					break;
			}
		}
	}
    return 0;
}


#define T1_BLOCK_RETRY    	3     		/* Number of retries */
#define NONIBLOCK   		0x80        /* Non I Block indicator */
#define SBLOCK      		0x40        /* S Block indicator     */
#define MOREBIT     		0x20        /* More bit indicator    */

#define SBLOCKFUNCMASK		0x3F    	/* S-block function mask */
#define CODESBLOCK(x)		(unsigned char)((x) | NONIBLOCK | SBLOCK)  /* Generates SBLOCK PCB BYTE */
#define CODERBLOCK(nr,rc)   (unsigned char)(NONIBLOCK | ((nr) << 4) | (rc)) /* Generates RBLOCK PCB BYTE */
#define CODEIBLOCK(ns,mb)   (unsigned char)(((ns) << 6) | ((mb) << 5)) /* Generates IBLOCK PCB BYTE */
#define ISIBLOCK(pcb)    	(!((pcb) & NONIBLOCK))       /* Returns TRUE if pcb belongs to I BLOCK */
#define ISRBLOCK(pcb)		((pcb & NONIBLOCK) && (!(pcb & SBLOCK)) && (!(pcb & MOREBIT)))  /* Returns TRUE if pcb belongs to R BLOCK */
#define ISSBLOCK(pcb)		(((pcb) & NONIBLOCK) && ((pcb) & SBLOCK)) /* Returns TRUE if pcb belongs to S BLOCK */
#define NR(pcb)				(((pcb) & 0x10) >> 4)		/* Retrieves R_Block Sequence number from pcb*/
#define NS(pcb)				(((pcb) & 0x40) >> 6)		/* Retrieves I_Block Sequence number from pcb*/
#define RERR(pcb)			((pcb) & 0x0F)			/* Retrieves Error field in R-blck pcb  */
#define MORE(pcb)			(((pcb) & MOREBIT) >> 5)	/* Retrieves More data bit info from pcb*/
#define SBLOCKFUNC(pcb)		((pcb) & SBLOCKFUNCMASK)	/* Retrieves S-block function code */
#define SREQUEST_ABORT		0xC2 /* S-block Abort operation function code */
#define SREQUEST_IFS		0xC1 /* S-block IFS Request function code */
#define SREQUEST_WTX		0xC3 /* S-block WTX Request function code */
#define SRESPONSE_IFS		0xE1 /* S-block IFS Response function code */
#define SRESPONSE_WTX		0xE3 /* S-block WTX Response function code */
#define SRESPONSE_ABORT		0xE2 /* S-block Abort Response function code */
#define generateEDC(onebyte, value) (value ^ onebyte)

static uint8_t wait_BWT = 0;
static uint32_t timeout;

void write_t1_byte(unsigned char onebyte, unsigned char timeout)
{
	icc->SCIBUFHW = onebyte;
	while (icc->SCISTAT&ICC_TX_FIFO_NOT_EMPTY){
		if (icc->SCISTAT & ICC_TX_ERROR) break;
	}
	if (timeout) {
		timer_stamp0 = get_sys_timer(); /* BWT */
		wait_BWT = 1;			
	}
	return;
}

signed short read_t1_byte(void)
{
	while(1) {
		if (!wait_BWT) {
			if ((get_sys_timer() - timer_stamp0) > g_atr[g_ipNum].CWT*COUNT_PER_ETU) return ERR_RECV_TIMEOUT;
		}
		else {
			if ((get_sys_timer() - timer_stamp0) > timeout*COUNT_PER_ETU) return ERR_RECV_TIMEOUT;
		}
		if ((icc->SCISTAT&ICC_RX_FIFO_NOT_EMPTY) != ICC_RX_FIFO_NOT_EMPTY) continue;
		timer_stamp0 = get_sys_timer();
		wait_BWT = 0;
		if((icc->SCISTAT&ICC_RX_ERROR) == ICC_RX_ERROR) {
		   t1_parity_error = 1;
		}
		return (int)icc->SCIBUFHW;
	}
}

void write_block(unsigned char NAD, unsigned char PCB, unsigned char length, unsigned char *buffer)
{
	unsigned char i = 0;
	unsigned char EDC = 0,len;

	len = length;
	icc->EDCDATA = 0;
	write_t1_byte(NAD, 0);
	write_t1_byte(PCB, 0);
	write_t1_byte(len, 0);

	for(i = 0; i < length; i++) {
		write_t1_byte(buffer[i], 0);
	}

	EDC = (unsigned char) icc->EDCDATA; /* calculate EDC by hardware */

	write_t1_byte(EDC, 1);

}

void send_block(unsigned char NAD, unsigned char PCB, unsigned char length, unsigned char *buffer)
{

	/* Calculate Wait Time Extension or Block Waiting time while receiving data
	   after sending last byte */
	if(g_atr[g_ipNum].wtx > 0)
		timeout = (1L* g_atr[g_ipNum].BWT * g_atr[g_ipNum].wtx);
	else
		timeout = g_atr[g_ipNum].BWT;

	wait_t1_BGT();

	write_block(NAD, PCB, length, buffer);
        
	return;
}

signed short receive_block(unsigned char *rNAD, unsigned char *rPCB, unsigned char *rLEN, unsigned char * buffer)
{
	signed short retval;
	signed short index = 0;
	signed short i;

	t1_parity_error=0;

	icc->EDCDATA = 0; /* clear EDCDTAT ,start EDC calculate */

	/* Get NAD, PCB, and Length */
	if ((retval = read_t1_byte()) < 0)
		goto exit_from_receiveblock;

	*rNAD = retval;

	if ((retval = read_t1_byte()) < 0)
		goto exit_from_receiveblock;

	*rPCB = retval;

	if ((retval = read_t1_byte()) < 0)
		goto exit_from_receiveblock;

	*rLEN = retval;
	/* Add additional byte if using CRC
  	   Get all data bytes plus EDC (1 byte at end) */
	for (i = 0; i <= *rLEN; i++) {
		if ((retval = read_t1_byte()) < 0)
			goto exit_from_receiveblock;
		buffer[index ++] = retval;
	}

	if(t1_parity_error) { /* check if there is any parity error */
		retval = ERR_RECV_PARITY;
		goto exit_from_receiveblock;
	}

		if (icc->EDCDATA != 0) {
		retval = ERR_RECV_LRC; /* Check if there is any LRC error */
		goto exit_from_receiveblock;
	}

	if (*rNAD != 0) { /* Check the received T1 block format */
		retval = ERR_INVALID_NAD;
		goto exit_from_receiveblock;
	}

	if((ISRBLOCK(*rPCB)&&(*rLEN!=0))||(ISSBLOCK(*rPCB)&&(*rLEN>1))) {
		retval= ERR_INVALID_LEN;
	}
	else {
		retval=*rLEN;
	}

exit_from_receiveblock:
	/* read URR to clear FIFO. we dont want to retain the data in FIFO while going to transmit mode */
	if ((icc->SCISTAT&ICC_RX_FIFO_NOT_EMPTY) == ICC_RX_FIFO_NOT_EMPTY) {
		icc->SCICTRL |= ICC_RX_FIFO_CLR;
	}
	icc->EDCDATA = 0;

	/* if length is FF, abort APDU transaction */
	if(*rLEN==0xFF) {
		return ERR_ABORT_T1;
	}

	return retval;
}

signed short send_sblock_IFSD(unsigned char IFSD)
{
	unsigned char buffer = IFSD;
	unsigned char rNAD, rPCB, rLEN;
	unsigned char rbuffer[256];
	unsigned char count = T1_BLOCK_RETRY;

	do
	{
		/*  S(IFS request) block with PCB = 'C1' and with IFSD = 254 */
		send_block(0, 0xC1, 1, &buffer);
		if((receive_block(&rNAD, &rPCB, &rLEN, rbuffer)) < 0) {
		  continue;
		}
		/* The PCB of the S(IFS response) block sent
		   in response shall have the value 'E1', and the INF field shall have the same
		   value as the INF field of the block requesting the change. */
		if ((rPCB == 0xE1) && (rLEN == 1) && (rbuffer[0] == IFSD))  {
		  return 0;
		}
	}while(--count);

	return ERR_SETIFSD_FAILURE;
}

#define set_NAD(nad)	g_atr[g_ipNum].NAD = (nad)

int receive_sblock(unsigned char *rNAD, unsigned char *rPCB, unsigned char *rLEN, unsigned char * rbuffer)
{
	int ret;
	int Responsed;

	t1_sblock_sent = 0;

	do {
		Responsed = 0;
		if((ret = receive_block(rNAD, rPCB, rLEN, rbuffer)) < 0) {
		    return ret;
		}

		switch (*rPCB) {
		case SREQUEST_ABORT : {
			/* Card requests to ABORT, send ABORT error code to abort transaction */
			return ERR_ABORT_T1;
		}
		case SREQUEST_IFS :
			/* Card sends IFSC request, Check if it is valid IFSC request */
			if((*rbuffer<0x10)||(*rbuffer==0xff)) {
				return ERR_INVALID_INF;
			}
			/* /Send IFSC response */
			send_block(0, SRESPONSE_IFS, 1, rbuffer);
			g_atr[g_ipNum].IFSC = *rbuffer;   /* Store new IFSC value in buffer */
			Responsed = 1; 	/* Set t1 status flags to maintain protocol state */
			t1_sblock_sent = 1;
			break;

		case SREQUEST_WTX :
			/* Card sends WTX Request */
			g_atr[g_ipNum].wtx = *rbuffer;       /* Store new WTX value in wtx buffer */
			send_block(0, SRESPONSE_WTX, 1, rbuffer); /* Send wtx response to card */
			/* After the ICC responds,
			BWT is again used as the time allowed for the ICC to process the I-block */
			g_atr[g_ipNum].wtx = 0;
			Responsed = 1;
			t1_sblock_sent = 1;
			break;
		}

	} while (Responsed);
	return ret;
}

int icc_send_apdu1(unsigned char *req_buf, unsigned short req_len, unsigned char *resp_buf)
{
	int ret;
	int currentIBlockLen, more, retry, i_block_retry;
	int totalResLength;
	unsigned char rNAD, rblock_sent=0;
	int cardiblock_err=0;
	unsigned char rPCB;
	unsigned char rLEN;
	unsigned char *IFDSeq, *ICCSeq, works[256];

	IFDSeq = &(g_atr[g_ipNum].IFD_Seq);
	ICCSeq = &(g_atr[g_ipNum].ICC_Seq);

	/* Make sure IFSD Negotiation takes place once before any APDU exchange for T1 protocol */
	if (g_atr[g_ipNum].IFSD_Negotiated == 0) {
		if (send_sblock_IFSD(0xFE) < 0) return ERR_SETIFSD_FAILURE;
		g_atr[g_ipNum].IFSD_Negotiated = 1;
	}
	/* Send APDU request */
	while (req_len) {

		/* We do chaining if request length is greater than IFSC */
		currentIBlockLen = (req_len < g_atr[g_ipNum].IFSC) ? req_len : g_atr[g_ipNum].IFSC;
		req_len -= currentIBlockLen;
		more = req_len ? 1 : 0;

		retry = T1_BLOCK_RETRY;

		i_block_retry = 1;
		cardiblock_err = 0;

		send_block(0, CODEIBLOCK(*IFDSeq, more), currentIBlockLen,req_buf);
		/* make sure block gets sent successfully within three times */
		while (retry--) {
			/* receive response from card for I-block */
			if ((ret = receive_sblock(&rNAD, &rPCB, &rLEN, works)) >= 0) {
				if((ISRBLOCK(rPCB))) { /* Check whether card sent R-Block */
					/* Check IFDseq to see whether wants to re-transmit previous block */
					if((NR(rPCB) == *IFDSeq) && (!t1_sblock_sent)) {
						if(((++i_block_retry) > 3)) {
							retry = -1;
							ret = -2;
							break;
						}
						send_block(0,CODEIBLOCK(*IFDSeq,more), currentIBlockLen, req_buf);
						retry ++;
						continue;
					}
					else  {
						/* Card sent ACK for previous i-block, send next chained i-block */
						if (more) {
							*IFDSeq = 1 - *IFDSeq;
							break;
						}
						else if(((!rblock_sent) && (retry < 2))) {
							/* there could be error in icc response to last i-block */
							send_block(0,
								CODERBLOCK(*ICCSeq, ((cardiblock_err == ERR_RECV_LRC) ||(cardiblock_err==ERR_RECV_PARITY)) ? 1 : 2), 0,(void*)0);
							retry ++;
							rblock_sent = 1;
							continue;
						}
					}
				}
				/* Card started sending APDU response, getinto receive APDU state machine */
				else if (ISIBLOCK(rPCB)) {
					if (((NS(rPCB) == *ICCSeq) && (more==0))) {
						*IFDSeq = 1 - *IFDSeq;
						break;
					}
				}
			}
			/* Card wants to abort T1 transaction, return with Abort Error Code */
			if(ret== ERR_ABORT_T1) return ret;
			if(retry) {
				if((cardiblock_err))
					ret = cardiblock_err;
				/* send R block saying we got invalid block */
				send_block(0,CODERBLOCK(*ICCSeq, ((ret==ERR_RECV_LRC)||(ret==ERR_RECV_PARITY)) ? 1 : 2), 0,(void*)0);
				cardiblock_err =ret;
			}
		}
		/* The transaction errors exceeded EMV limit, so, return error code */
		if (retry < 0) return ERR_MAX_RETRIED;
		req_buf += currentIBlockLen;
	}
	totalResLength = 0;

	do {
		memcpy(resp_buf, works, rLEN);
		resp_buf += rLEN;
		totalResLength += rLEN;

		*ICCSeq = 1 - *ICCSeq;
		/* Check whether there are more chained APDU responses blocks */
		if (MORE(rPCB)) {
			send_block(0,CODERBLOCK(*ICCSeq, 0), 0, (void*)0); /* Send ACK for received I-block */
			retry = T1_BLOCK_RETRY;

			while (retry) {
				ret = receive_sblock(&rNAD, &rPCB, &rLEN, works); /* Receive Next Block from Card */

				if(ret >= 0) {
					if (ISRBLOCK(rPCB)) { /* Card did not receive our ACK properly, resend it */
						if (!(NR(rPCB) != *IFDSeq)) {
							send_block(0,CODERBLOCK(*ICCSeq, 0), 0,(void*)0);
							continue;
						}
					}
					else if (ISIBLOCK(rPCB)) {
						if (NS(rPCB) == *ICCSeq) break;
					}
				}

				if(ret== ERR_ABORT_T1) return ret;

				if(--retry)  send_block(0,CODERBLOCK(*ICCSeq, 0), 0,(void*)0);
			}

			if (!retry) return ERR_MAX_RETRIED;
		}
		else
		{
			break;
		}
	} while (1);
	return totalResLength;
}

static int icc_send_apdu(unsigned char *buffer, unsigned int length, unsigned char *rbuffer)
{
	int ret = -1;
    
	switch (g_atr[g_ipNum].T) {
	case 0:
		ret = icc_send_apdu0(buffer,length,rbuffer);
		break;
	case 1:
		ret = icc_send_apdu1(buffer,length,rbuffer);
		break;
	default:
		break;
	}
  
	return ret;
}

static int icc_powerup(unsigned char * atrstr, int buflen)
{
	int ret = 0;
	
	if (icc_present()) {
		apb7816_powerup();

		if ((ret = icc_atr_sequence(0, atrstr, buflen)) < 0) {
			return ret;
		}
		if (ret == 0) {
			icc_warm_reset();
			if ((ret = icc_atr_sequence(1, atrstr, buflen)) < 0) {
				return ret;
			}
		}
	}
	else {
		return -1;
	}

	return ret;
}

static int iso_init(iodev_t * dev)
{
    int temp;
    icc_ex_t ex;
    
    auto_poweronoff_disable();
    temp = g_ipNum;
    g_ipNum = 0;
    if (icc_present()) {
        ip7816_init();
        ex.resp = icc_powerup(ex.exbuf, EX_BUF_LEN);
        icc_powerdown();
    }
    g_ipNum = temp;

    return 0;
}

static int icc_ioctl(iodev_t *dev, uint32_t cmd, void *arg)
{
        switch(cmd) {
        case IOCTL_ICC_SELECT_SLOT:
                {
                        uint32_t slot = (uint32_t)arg;
                        if (slot > IP2) return -1;
                        g_ipNum = slot;
                        icc = (icc_reg_t*)(0xba020000 + g_ipNum*0x10000);
                        if (g_ipNum == IP0) icc_timer_factor = cp0info_cur_freq()/CONFIG_ICC0_CLK;
                        else if (g_ipNum == IP1) icc_timer_factor = cp0info_cur_freq()/CONFIG_ICC1_CLK;
                        else if (g_ipNum == IP2) icc_timer_factor = cp0info_cur_freq()/CONFIG_ICC2_CLK;
                }
                break;
        case IOCTL_ICC_POWERUP:
                {
                        icc_ex_t ex;
                        ip7816_init();
                        __disable_interrupt();
                        ex.resp = icc_powerup(ex.exbuf, EX_BUF_LEN);
                        __enable_interrupt();
                        if (arg) {
                                memcpy((void *) arg, (void*)&ex, sizeof (ex));
                        }
                        if (ex.resp < 0) {
                                icc_powerdown();
                                return -1;
                        }
                }
                break;
        case IOCTL_ICC_POWERDOWN:
                {
                        icc_powerdown();
                }
                break;
        case IOCTL_ICC_APDU:
                if (arg) {
                        icc_ex_t ex;
                        memcpy(&ex, arg, sizeof(ex));
                        if (ex.inlen) {
                                __disable_interrupt();
                                ex.resp = icc_send_apdu(ex.exbuf, ex.inlen, ex.exbuf);
                                __enable_interrupt();
                                memcpy((void *) arg, (void*)&ex, sizeof (ex));
                                if (ex.resp < 0) {
                                        icc_powerdown();
                                        return -1;
                                }
                        }
                }
                break;
        default:
                break;
        }
        return 0;
}

static int icc_poll(iodev_t *dev, uint32_t flags)
{
        if (flags & (POLL_READ | POLL_WRITE)) {
                if (!icc_present()) {
                        flags &= ~(POLL_READ | POLL_WRITE);
                }
        }

        return flags;
}

static const iodev_t icc_iso __iodev_entry = {
        .name = "icc",
        .init = iso_init,
        .read = NULL,
        .write = NULL,
        .ioctl = icc_ioctl,
        .poll = icc_poll,
        .wakeup = NULL,
        .suspend = NULL,
};

